/*
 * GPS.h
 *
 *  Created on: Feb 9, 2014
 *      Author: ppks
 */

#ifndef GPS_H_
#define GPS_H_

/********************************************************************************/
/* Constants Definition			                                                */
/********************************************************************************/
#define GPS_STABLE_MIN_SATELLITE_NUM    5

#define GPS_FILTER_VECTOR_LENGTH        5

/* Offsets in coordinates array */
#define LAT                             0x00
#define LON                             0x01

#define X_AXIS                          LON
#define Y_AXIS                          LAT

#if (GPS_PROTOCOL == UBLOX)
    #define GPS_LAG                     0.5f    /* UBLOX GPS has a smaller lag than MTK and other */
#else  /* (GPS_PROTOCOL == UBLOX) */
  #define GPS_LAG                       1.0f    /* We assumes that MTK GPS has a 1 sec lag */
#endif /* (GPS_PROTOCOL == UBLOX) */

#define RADX                            0.0174532925f
#define RADX100                         0.000174532925f
#define NAV_POSHOLD_MAX_SPEED           100     /* Maximum speed in Position Hold mode in cm/s */

/* Navigation Constatns */
#define NAV_MAX_WAYPOINTS_NUM           254

#define GPS_MODE_NONE                   0       /* This is the mode what is selected via the remote (NONE, HOLD, RTH and NAV (NAV-> exectute mission) */
#define GPS_MODE_HOLD                   1
#define GPS_MODE_RTH                    2
#define GPS_MODE_NAV                    3

/* GPS Navigation State Machine States Definition */
#define NAV_STATE_NONE                  0
#define NAV_STATE_RTH_START             1
#define NAV_STATE_RTH_ENROUTE           2
#define NAV_STATE_HOLD_INFINIT          3
#define NAV_STATE_HOLD_TIMED            4
#define NAV_STATE_WP_ENROUTE            5
#define NAV_STATE_PROCESS_NEXT          6
#define NAV_STATE_DO_JUMP               7
#define NAV_STATE_LAND_START            8
#define NAV_STATE_LAND_IN_PROGRESS      9
#define NAV_STATE_LANDED                10
#define NAV_STATE_LAND_SETTLE           11
#define NAV_STATE_LAND_START_DESCENT    12

/* Navigation Erros Definition */
#define NAV_ERROR_NONE                  0       /* All systems clear */
#define NAV_ERROR_TOOFAR                1       /* Next waypoint distance is more than safety distance */
#define NAV_ERROR_SPOILED_GPS           2       /* GPS reception is compromised - Nav paused - copter is adrift ! */
#define NAV_ERROR_WP_CRC                3       /* CRC error reading WP data from EEPROM - Nav stopped */
#define NAV_ERROR_FINISH                4       /* End flag detected, navigation finished */
#define NAV_ERROR_TIMEWAIT              5       /* Waiting for poshold timer */
#define NAV_ERROR_INVALID_JUMP          6       /* Invalid jump target detected, aborting */
#define NAV_ERROR_INVALID_DATA          7       /* Invalid mission step action code, aborting, copter is adrift */
#define NAV_ERROR_WAIT_FOR_RTH_ALT      8       /* Waiting to reach RTH Altitude */
#define NAV_ERROR_GPS_FIX_LOST          9       /* Gps fix lost, aborting mission */
#define NAV_ERROR_DISARMED              10      /* NAV engine disabled due disarm */
#define NAV_ERROR_LANDING               11

/* Possible action codes for a mission step */
#define MISSION_ACTION_NONE             0
#define MISSION_WAYPOINT                1       /* Set waypoint */
#define MISSION_HOLD_UNLIM              2       /* Poshold unlimited */
#define MISSION_HOLD_TIME               3       /* Hold for a predetermined time */
#define MISSION_RTH                     4       /* Return to HOME */
#define MISSION_SET_POI                 5       /* Set POINT of interest */
#define MISSION_JUMP                    6       /* Jump to the given step (#times) */
#define MISSION_SET_HEADING             7       /* Set heading to a given orientation (parameter 1 is the waym 0-359 degree */
#define MISSION_LAND                    8       /* Land at the given position */

/* Mission flags definition */
#define MISSION_FLAG_END                0xA5    /* Flags that this is the last step */
#define MISSION_FLAG_CRC_ERROR          0xFE    /* Returned WP had an EEPROM CRC error */
#define MISSION_FLAG_HOME               0x01    /* Returned WP is the home position */
#define MISSION_FLAG_HOLD               0x02    /* Returned WP is the hold position */
#define MISSION_FLAG_DO_LAND            0x20    /* Land when reached desired point (used in RTH) */
#define MISSION_FLAG_NAV_IN_PROG        0xFF    /* Navigation is in progress, returned WP is home */

/* Altitude change flag */
#define ALT_FLAG_ASCENDING              1
#define ALT_FLAG_DESCENDING             -1
#define ALT_FLAG_REACHED_ALT            0

#define NAV_LAND_DETECT_THRESHOLD       40
#define NAV_LAND_BAROPIDMIN             -180    /* BaroPID reach this if we landed..... */
#define NAV_LAND_MIN_SPEED              20

#define GPS_LOST_SIGNAL_TIMEOUT_MS      1200
#define GPS_SETTLE_TIME_BEFORE_LAND_MS  5000
#define GPS_DISARM_TIME_AFTER_LAND_MS   5000

/********************************************************************************/
/* Types Definition                                                             */
/********************************************************************************/
 typedef struct {
    uint8_t   number;     /* Waypoint number */
    uint8_t   action;     /* Action to follow */
    int32_t   pos[2];     /* GPS position */
    uint32_t  altitude;   /* Altitude in cm (AGL) */
    int16_t   parameter1; /* Parameter for the action */
    int16_t   parameter2; /* Parameter for the action */
    int16_t   parameter3; /* Parameter for the action */
    uint8_t   flag;       /* flags the last wp and other fancy things that are not yet defined */
    uint8_t   checksum;   /* this must be at the last position */
} WaypointRecord_t;

/********************************************************************************/
/* Exported Variables Declaration                                               */
/********************************************************************************/
extern uint8_t  GPS_Enabled;               /* Flag that tells whether the GPS is enabled or not */
extern uint8_t  GPS_Present;
extern uint8_t  GPS_Update;                /* a binary toogle to distinct a GPS position update */
extern uint8_t  GPS_NumSat;                /* A number of detected satellites */

extern int32_t  GPS_CurrentCoord[];        /* Current GPS coordinates - longitude and latitude */
extern int32_t  GPS_HomeCoord[];           /* Coordinates of home location */
extern int32_t  GPS_HoldCoord[];           /* Coordinates for hold position */
extern int32_t  GPS_NextWPCoord[];         /* Next WP Coordinates when in NAV mode */
extern int32_t  GPS_PrevWPCoord[2];

extern uint16_t GPS_DistanceToHome;        /* Distance to home  - unit: meter */
extern int16_t  GPS_DirectionToHome;       /* Direction to home - unit: degree */
extern uint16_t GPS_Altitude;              /* GPS altitude      - unit: meter */
extern uint16_t GPS_Speed;                 /* GPS speed         - unit: cm/s */
extern uint16_t GPS_GroundCourse;          /* GPS Ground Course - unit: degree * 10 */
extern int16_t  GPS_Angle[];               /* The angles that must be applied for GPS correction */

extern WaypointRecord_t WaypointRecord;

extern uint8_t  NAV_state;
extern uint8_t  NAV_error;

extern int16_t  NAV_Bank[2];                   /* The desired bank towards North (Positive) or South (Negative) : latitude & longitude */
extern int32_t  GPS_TargetBearing;             /* This is the angle from the copter to the "next_WP" location in degrees * 100 */
extern int32_t  GPS_OriginalTargetBearing;     /* Crosstrack: deg * 100, The original angle to the next_WP when the next_WP was set */
extern int16_t  GPS_TakeoffBearing;            /* saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home */
extern int16_t  GPS_CosstrackError;            /* The amount of angle correction applied to target_bearing to bring the copter back on its optimum path */
extern uint32_t GPS_DistanceToWP;              /* distance between plane and next_WP in cm */

extern uint8_t  NAV_PauseAtStep;
extern uint8_t  NAV_NextStep;
extern int16_t  NAV_JumpTimes;

/********************************************************************************/
/* Exported Functions Prototypes                                                */
/********************************************************************************/
extern void GPS_Init(void);
extern void GPS_GetNewData(void);
extern void GPS_ResetHomePosition(void);
extern void GPS_InitRTH(void);
extern void GPS_CalculateAngles(void);
extern void GPS_SetNextWP(int32_t* newWPCoord, int32_t* currentWPCoord);
extern void GPS_ResetNavigation(void);
extern int32_t GPS_GetNewAltitude(void);
extern void GPS_SetNewAltitude(int32_t New_Altitude);

#endif /* __GPS_H_ */
